#ifndef DRIVETRAIN_H
#define DRIVETRAIN_H

#include <ros/ros.h>
#include<WarbotsMsg/posUpdateMsg.h>
#include<WarbotsMsg/manualCtrlMsg.h>
#include <cstddef>
#include <math.h>
#include <cmath>
#include <iostream>
#include <vector>
#include "common.h"

class drivetrain
{
   public:
      drivetrain()
      {
         rParam.numOfWheels = NUM_OF_WHEELS;
         rParam.maxSpd = MAX_SPD;
         rParam.rWheels = RADIUS_OF_WHEELS;
         rParam.l = LENGTH;
         rParam.payload = PAYLOAD;
         for(int i=0; i<rParam.numOfWheels; i++)
         {
            rParam.alpha[i] = (double)(PI*0.25+i*PI*0.5);
            rParam.rRotSpd[i] = 0;
         }
         nextWaypt.pos.x = 0;
         nextWaypt.pos.y = 0;
         nextWaypt.heading = 0;
         Mode = PAUSED;
         initialized = false;
      }

      /*--------- public member functions ---------*/
      void reset();
      DriveTrainMode_t getMode();
      ErrCode_t start();
      void stop();
      ErrCode_t set_traj(waypt_t* nWaypt, int size, bool reset);   
      void display_traj();
      void vehicle_posUpdate(const WarbotsMsg::posUpdateMsg& msg);
      void set_wheelRotSpd(double* set_spd);
      void manual_control(const WarbotsMsg::manualCtrlMsg& msg, double* set_spd);
     

   private:
      bool initialized;
      DriveTrainMode_t Mode;

      struct rParam_t
      {
         int numOfWheels;
         double rWheels;
         double rWeight;
         double maxSpd;
         double payload;
         double l;
         double alpha[NUM_OF_WHEELS];
         double rRotSpd[NUM_OF_WHEELS];
      }rParam;

      struct rState_t
      {
         pos_t pos;
         spd_t spd;
         double heading;
      }rState;

      waypt_t nextWaypt;
      std::vector<waypt_t> traj;
      

      /*--------- private member functions ---------*/
      double* get_wheelRotSpd();
      bool atWaypt();
      waypt_t get_next_waypt();
};

#endif
